home *** CD-ROM | disk | FTP | other *** search
/ Amiga Format CD 49 / Amiga Format CD49 (2000-01-17)(Future Publishing)(GB)(Track 1 of 3)[!][issue 2000-02].iso / -serious- / graphics / gnuplot / gnuplot-3.7.1src / gnuplot-3.7.1 / matrix.c < prev    next >
C/C++ Source or Header  |  1999-11-29  |  7KB  |  269 lines
  1. #ifndef lint only calculate Sid = "$Id: matrix.c,v 1.12 1998/11/19 10:40:31 lhecking Exp $";
  2. #endif
  3.  
  4. /*
  5.  *    Matrix algebra, part of
  6.  *
  7.  *    Nonlinear least squares fit according to the
  8.  *    Marquardt-Levenberg-algorithm
  9.  *
  10.  *    added as Patch to Gnuplot (v3.2 and higher)
  11.  *    by Carsten Grammes
  12.  *    Experimental Physics, University of Saarbruecken, Germany
  13.  *
  14.  *    Internet address: cagr@rz.uni-sb.de
  15.  *
  16.  *    Copyright of this module:   Carsten Grammes, 1993
  17.  *
  18.  *    Permission to use, copy, and distribute this software and its
  19.  *    documentation for any purpose with or without fee is hereby granted,
  20.  *    provided that the above copyright notice appear in all copies and
  21.  *    that both that copyright notice and this permission notice appear
  22.  *    in supporting documentation.
  23.  *
  24.  *    This software is provided "as is" without express or implied warranty.
  25.  */
  26.  
  27.  
  28. #include "fit.h"
  29. #include "matrix.h"
  30. #include "stdfn.h"
  31. #include "alloc.h"
  32.  
  33. /*****************************************************************/
  34.  
  35. #define Swap(a,b)   {double temp = (a); (a) = (b); (b) = temp;}
  36. #define WINZIG          1e-30
  37.  
  38.  
  39. /*****************************************************************
  40.     internal prototypes
  41. *****************************************************************/
  42.  
  43. static GP_INLINE int fsign __PROTO((double x));
  44.  
  45. /*****************************************************************
  46.     first straightforward vector and matrix allocation functions
  47. *****************************************************************/
  48. double *vec (n)
  49. int n;
  50. {
  51.     /* allocates a double vector with n elements */
  52.     double *dp;
  53.     if( n < 1 )
  54.     return (double *) NULL;
  55.     dp = (double *) gp_alloc ( n * sizeof(double), "vec");
  56.     return dp;
  57. }
  58.  
  59.  
  60. double **matr (rows, cols)
  61. int rows;
  62. int cols;
  63. {
  64.     /* allocates a double matrix */
  65.  
  66.     register int i;
  67.     register double **m;
  68.  
  69.     if ( rows < 1  ||  cols < 1 )
  70.         return NULL;
  71.     m = (double **) gp_alloc ( rows * sizeof(double *) , "matrix row pointers");
  72.     m[0] = (double *) gp_alloc ( rows * cols * sizeof(double), "matrix elements");
  73.     for ( i = 1; i<rows ; i++ )
  74.         m[i] = m[i-1] + cols;
  75.     return m;
  76. }
  77.  
  78.  
  79. void free_matr (m)
  80. double **m;
  81. {
  82.     free (m[0]);
  83.     free (m);
  84. }
  85.  
  86.  
  87. double *redim_vec (v, n)
  88. double **v;
  89. int n;
  90. {
  91.     if ( n < 1 ) 
  92.       *v = NULL;
  93.     else
  94.       *v = (double *) gp_realloc (*v, n * sizeof(double), "vec");
  95.     return *v;
  96. }
  97.  
  98. void redim_ivec (v, n)
  99. int **v;
  100. int n;
  101. {
  102.     if ( n < 1 ) {
  103.     *v = NULL;
  104.     return;
  105.     }
  106.     *v = (int *) gp_realloc (*v, n * sizeof(int), "ivec");
  107. }
  108.  
  109.  
  110. /* HBB: TODO: is there a better value for 'epsilon'? how to specify
  111.  * 'inline'?  is 'fsign' really not available elsewhere? use
  112.  * row-oriented version (p. 309) instead?
  113.  */
  114.  
  115. static GP_INLINE int fsign(x)
  116.   double x;
  117. {
  118.     return( x>0 ? 1 : (x < 0) ? -1 : 0) ;
  119. }
  120.  
  121. /*****************************************************************
  122.  
  123.      Solve least squares Problem C*x+d = r, |r| = min!, by Given rotations
  124.      (QR-decomposition). Direct implementation of the algorithm
  125.      presented in H.R.Schwarz: Numerische Mathematik, 'equation'
  126.      number (7.33)
  127.  
  128.      If 'd == NULL', d is not accesed: the routine just computes the QR
  129.      decomposition of C and exits.
  130.  
  131.      If 'want_r == 0', r is not rotated back (\hat{r} is returned
  132.      instead).
  133.  
  134. *****************************************************************/
  135.  
  136. void Givens (C, d, x, r, N, n, want_r)
  137. double **C;
  138. double *d;
  139. double *x;
  140. double *r;
  141. int N;
  142. int n;
  143. int want_r;
  144. {
  145.     int i, j, k;
  146.     double w, gamma, sigma, rho, temp;
  147.     double epsilon = DBL_EPSILON; /* FIXME (?)*/
  148.  
  149. /* 
  150.  * First, construct QR decomposition of C, by 'rotating away'
  151.  * all elements of C below the diagonal. The rotations are
  152.  * stored in place as Givens coefficients rho.
  153.  * Vector d is also rotated in this same turn, if it exists 
  154.  */
  155.     for (j = 0; j<n; j++) 
  156.         for (i = j+1; i<N; i++) 
  157.             if (C[i][j]) {
  158.                 if (fabs(C[j][j])<epsilon*fabs(C[i][j])) { /* find the rotation parameters */
  159.                     w = -C[i][j];
  160.                     gamma = 0;
  161.                     sigma = 1;
  162.                     rho = 1;
  163.         } else {
  164.             w = fsign(C[j][j])*sqrt(C[j][j]*C[j][j] + C[i][j]*C[i][j]);
  165.             if (w == 0)
  166.             Eex3 ( "w = 0 in Givens();  Cjj = %g,  Cij = %g", C[j][j], C[i][j]);
  167.             gamma = C[j][j]/w;
  168.             sigma = -C[i][j]/w;
  169.             rho = (fabs(sigma)<gamma) ? sigma : fsign(sigma)/gamma;
  170.         }
  171.         C[j][j] = w;
  172.         C[i][j] = rho;           /* store rho in place, for later use */
  173.         for (k = j+1; k<n; k++) {   /* rotation on index pair (i,j) */
  174.             temp =    gamma*C[j][k] - sigma*C[i][k];
  175.             C[i][k] = sigma*C[j][k] + gamma*C[i][k];
  176.             C[j][k] = temp;
  177.             
  178.         }
  179.         if (d) {               /* if no d vector given, don't use it */
  180.             temp = gamma*d[j] - sigma*d[i];  /* rotate d */
  181.             d[i] = sigma*d[j] + gamma*d[i];
  182.             d[j] = temp;
  183.             }
  184.         }
  185.     if (!d)               /* stop here if no d was specified */
  186.          return;
  187.  
  188.     for (i = n-1; i >= 0; i--) {   /* solve R*x+d = 0, by backsubstitution */
  189.         double s = d[i];
  190.         r[i] = 0;              /* ... and also set r[i] = 0 for i<n */
  191.         for (k = i+1; k<n; k++) 
  192.             s += C[i][k]*x[k];
  193.     if (C[i][i] == 0)
  194.       Eex ( "Singular matrix in Givens()");
  195.         x[i] = - s / C[i][i];
  196.     }
  197.     for (i = n; i < N; i++) 
  198.         r[i] = d[i];             /* set the other r[i] to d[i] */
  199.         
  200.     if (!want_r)            /* if r isn't needed, stop here */
  201.         return;
  202.         
  203.     /* rotate back the r vector */
  204.     for (j = n-1; j >= 0; j--)
  205.         for (i = N-1; i >= 0; i--) {
  206.             if ((rho = C[i][j]) == 1) { /* reconstruct gamma, sigma from stored rho */
  207.                  gamma = 0;
  208.                  sigma = 1;
  209.             } else if (fabs(rho)<1) {
  210.                 sigma = rho; 
  211.                 gamma = sqrt(1-sigma*sigma);
  212.             } else {
  213.                 gamma = 1/fabs(rho);
  214.                 sigma = fsign(rho)*sqrt(1-gamma*gamma);
  215.             }
  216.         temp = gamma*r[j] + sigma*r[i];    /* rotate back indices (i,j) */
  217.         r[i] = -sigma*r[j] + gamma*r[i];
  218.         r[j] = temp;
  219.     }
  220. }
  221.  
  222.  
  223. /* Given a triangular Matrix R, compute (R^T * R)^(-1), by forward
  224.  * then back substitution
  225.  * 
  226.  * R, I are n x n Matrices, I is for the result. Both must already be
  227.  * allocated.
  228.  * 
  229.  * Will only calculate the lower triangle of I, as it is symmetric 
  230.  */
  231.  
  232. void Invert_RtR ( R, I, n)
  233. double **R;
  234. double **I;
  235. int n;
  236. {
  237.   int i, j, k;
  238.  
  239.   /* fill in the I matrix, and check R for regularity : */
  240.  
  241.   for (i = 0; i<n; i++) {
  242.     for (j = 0; j<i; j++)  /* upper triangle isn't needed */
  243.       I[i][j] = 0;
  244.     I[i][i] = 1;
  245.     if (! R[i][i])
  246.       Eex ("Singular matrix in Invert_RtR")
  247. }
  248.  
  249.   /* Forward substitution: Solve R^T * B = I, store B in place of I */
  250.  
  251.   for (k = 0; k<n; k++) 
  252.     for (i = k; i<n; i++) {  /* upper half needn't be computed */
  253.       double s = I[i][k];
  254.       for (j = k; j<i; j++)  /* for j<k, I[j][k] always stays zero! */
  255.     s -= R[j][i] * I[j][k];
  256.       I[i][k] = s / R[i][i];
  257. }
  258.  
  259.   /* Backward substitution: Solve R * A = B, store A in place of B */
  260.  
  261.   for (k = 0; k<n; k++)
  262.     for (i = n-1; i >= k; i--) {  /* don't compute upper triangle of A */
  263.       double s = I[i][k];
  264.       for (j = i+1; j<n; j++)
  265.     s -= R[i][j] * I[j][k];
  266.       I[i][k] = s / R[i][i]; 
  267.     }
  268. }
  269.